#include <rclcpp/rclcpp.hpp>
#include <rclcpp/qos.hpp>  // 添加此行以包含QoS设置
#include <sensor_msgs/msg/laser_scan.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <cmath>

using namespace std::chrono_literals;

class LidarVisualizerNode : public rclcpp::Node {
public:

    LidarVisualizerNode() : Node("lidar_visualizer") {
        
        // 显式设置可靠传输的QoS策略
        rclcpp::QoS qos(rclcpp::KeepLast(10));
        qos.reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT);
        // 订阅激光雷达的 /scan 话题
        scan_sub_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
            "/scan", qos, std::bind(&LidarVisualizerNode::scanCallback, this, std::placeholders::_1));
        // 设置窗口名称
        cv::namedWindow("Lidar Visualization", cv::WINDOW_AUTOSIZE);
    }

    ~LidarVisualizerNode() {
        cv::destroyWindow("Lidar Visualization");
    }

private:
    // 激光数据可视化回调函数 
    void scanCallback(const sensor_msgs::msg::LaserScan::SharedPtr scan_msg) {
        // 初始化图像参数
        int image_size = 1000;
        float resolution = 100;  // 图像的分辨率（1米中包含的像素点）
        cv::Mat image(image_size, image_size, CV_8UC3, cv::Vec3b(255, 255, 255));
        cv::Vec3b color(0, 0, 255);  // 红色点表示激光数据

        // 调用内部的可视化函数
        Visualize2DScan(scan_msg, image, color, image_size, resolution);

        // 显示图像
        cv::imshow("Lidar Visualization", image);
        cv::waitKey(1);
    }

    // 封装的Visualize2DScan方法
    void Visualize2DScan(const sensor_msgs::msg::LaserScan::SharedPtr scan, cv::Mat& image,
                         const cv::Vec3b& color, int image_size, float resolution) {
        // 遍历激光扫描的每一个数据点
        for (size_t i = 0; i < scan->ranges.size(); ++i) {
            // 检查扫描范围是否有效
            if (scan->ranges[i] < scan->range_min || scan->ranges[i] > scan->range_max) {
                continue; // 如果无效，跳过该点
            }

            // 计算实际的激光角度
            double real_angle = scan->angle_min + i * scan->angle_increment;
            // 计算(x, y)坐标
            double x = scan->ranges[i] * std::cos(real_angle) ;
            double y = scan->ranges[i] * std::sin(real_angle) ;
            // 过滤掉不在指定角度范围内的激光束（±30度）
            if (real_angle < scan->angle_min + 30 * M_PI / 180.0 || real_angle > scan->angle_max - 30 * M_PI / 180.0) {
                continue; // 如果不在范围内，跳过该点
            }
            // 计算图像坐标
            int image_x = int(x * resolution + image_size / 2);
            int image_y = int(y * resolution + image_size / 2);
            // 检查计算出的图像坐标是否在图像范围内
            if (image_x >= 0 && image_x < image.cols && image_y >= 0 && image_y < image.rows) {
                // 在图像中设置对应像素的颜色
                image.at<cv::Vec3b>(image_y, image_x) = cv::Vec3b(color[0], color[1], color[2]);
            }
        }

        // 绘制传感器自身位置
        cv::circle(image, cv::Point2f(image_size / 2, image_size / 2), 5, cv::Scalar(color[0], color[1], color[2]), 2);
    }

    rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_sub_;
};

int main(int argc, char **argv) {
    // 初始化ROS2节点
    rclcpp::init(argc, argv);
    auto node = std::make_shared<LidarVisualizerNode>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}
